#include "CommunicationsHandler.h"
#include "LCD_Handler.h"

#pragma MESSAGE DISABLE C3401
#pragma MESSAGE DISABLE C1825
#pragma MESSAGE DISABLE C4000

const uint8 u8M_ERROR1[16] = {"     ERROR      "};
const uint8 u8M_ERROR2[16] = {" Sensor manager "};
const uint8 u8M_ERROR3[16] = {" did not answer "};
const uint8 u8M_ERROR4[16] = {"to start message"};

const uint8 u8M_ERROR5[16] = {"     ERROR      "};
const uint8 u8M_ERROR6[16] = {"  Acceleration  "};
const uint8 u8M_ERROR7[16] = {" sensor is not  "};
const uint8 u8M_ERROR8[16] = {"working properly"};


uint8 u8Comm_SensorPacket [COMM_SENSOR_PACKET_SIZE];
uint8 u8Comm_TxSMACPacket [COMM_TX_SMAC_PACKET_SIZE];
uint8 u8Comm_RxSMACPacket [COMM_RX_SMAC_PACKET_SIZE];

extern uint16 u16DistanceToAirplane;
extern uint8 u8Directions;
extern uint8 u8PlaneSpeed;	
extern uint16 u16Altitude;
extern uint8 u8batteryPlane;
extern uint8 u8Ambientemperature;
extern uint8 u8AirplaneGeneralStatus;
extern uint8 u8PlaneConfAnswerFailure;
extern uint16 u16PlaneConfAnswer;
extern uint8 u8ControllerStatus;
extern uint8 u8ExtraAirplaneSettings;

extern uint8 u8configuration1;
extern uint8 u8configuration2;
extern uint8 u8configuration3;

uint8 u8Comm_CurrentSMACMsg = 0;

uint16 u16Comm_CurrentMACAddr = 0x1234;

extern uint8 u8accel_x;
extern uint8 u8accel_y;
extern uint8 u8accel_z;
extern uint8 u8speed;
extern uint8 u8battery;
extern uint8 u8buttons;

extern uint8 u8LCD_Buffer1[16];
extern uint8 u8LCD_Buffer2[16];
extern uint8 u8LCD_Buffer3[16];
extern uint8 u8LCD_Buffer4[16];

void vfnComm_PutRequestOnSmacPacket(uint16 value)
{
 	if( value & 0x00F0 )	
 	{
 		u8configuration3 = 0;
 		u8configuration2 = 0;
 		u8configuration1 = 0xF0 | ( value >> 4) & 0x000F; 		
 	}
 	else if( value & 0x000F )
 	{
 		u8configuration1 = 0;
 		u8configuration2 = 0xF0 | ( value & 0x000F); 		
 		u8configuration3 = 0;
 	}
 	else if( value & 0x3F00 )
 	{
 		u8configuration1 = 0;
 		u8configuration3 = 0xC0 | ( value >> 8 )& 0x003F; 		
 		u8configuration2 = 0;
 	}
 	
 	u8PlaneConfAnswerFailure = 0;
	u16PlaneConfAnswer = 0;
}

void vfnComm_ConfigureSCI (void)
{
    vfnSCI_Init ();
	vfnSCI_SetBaudRate (SCI_38400BAUD);
}

void vfn_CommSayHelloSensorManager(void)
{
	uint8 u8tempo = 255;
	(void)u8SCI1_RxPending();
    (void)u8SCI1_ReadByte();
    
    while(--u8tempo)
    {
    	vfnLCD_Delay(DELAY_2MS);
    }
    
    u8tempo = 255;
    
    vfnSCI1_SendByte (COMM_COMMAND_HELLO);

    while(!u8SCI1_TxBufferEmpty());
    
    while(!u8SCI1_RxPending() && u8tempo)
    {   
    	vfnLCD_Delay(DELAY_2MS);
    	--u8tempo;
    };
    
    if(u8tempo)
	{
		if(u8SCI1_ReadByte() == COMM_ANSWER_HELLO_OK)
			u8tempo = 1;
		else
		if(u8SCI1_ReadByte() == COMM_ANSWER_HELLO_ACCEL_FAIL)
		{
			vfnLCD_putBuffer1(u8M_ERROR5);
	    	vfnLCD_putBuffer2(u8M_ERROR6);
	    	vfnLCD_putBuffer3(u8M_ERROR7);
	    	vfnLCD_putBuffer4(u8M_ERROR8);
	    	u8tempo = 0;
		}
	}
	else
	{
		vfnLCD_putBuffer1(u8M_ERROR1);
    	vfnLCD_putBuffer2(u8M_ERROR2);
    	vfnLCD_putBuffer3(u8M_ERROR3);
    	vfnLCD_putBuffer4(u8M_ERROR4);
    	
	}    
    
    if(!u8tempo)
    {
    	vfn_LCD_MenuSelLine(1);
    	vfnLCD_SendLine(1, u8LCD_Buffer1);
		vfnLCD_SendLine(2, u8LCD_Buffer2);
		vfnLCD_SendLine(3, u8LCD_Buffer3);
		vfnLCD_SendLine(4, u8LCD_Buffer4);
    	while(1);	//Do not continue
	}   	   	
}

void vfnComm_ReadSCIPacket (void)
{
    uint8 u8tempo;
    static uint8 u8LastBattery;
    uint16 u16tempo;
    u8accel_x = u8Comm_SensorPacket[0];
    
    u8accel_y = u8Comm_SensorPacket[1];
    
    u8accel_z = u8Comm_SensorPacket[2];
    
    u8speed = u8Comm_SensorPacket[3];

    if(u8LastBattery < u8Comm_SensorPacket[4]) //One way hysteresis for battery
    {
    	u8tempo = u8Comm_SensorPacket[4] - u8LastBattery;
    }
    else
    {
    	u8tempo = 2;
    }
    
    if(u8tempo>1)
	{    
	    u8tempo=u8Comm_SensorPacket[4];
	    u8LastBattery = u8Comm_SensorPacket[4];
	    
	    if(u8tempo<174)
	    {
	    	u8battery=0;
	    }
	    else if(u8tempo>239)
	    {
	    	u8battery=100;
	    }
	    else
	    {
	    	u8tempo = u8tempo - 174;
	    	u16tempo = u8tempo * 100;
	    	u8battery = (uint8)(u16tempo / 65);	
	    }
    }
    else
    {
    	//Level is no updated		
    }
    
    u8buttons = u8Comm_SensorPacket[5] | ( u8buttons & 0x0F );	
    	//Unnatended button events are'n affected by this function. However, current pressed buttons are
    	//updated every time this function is executed
            
}

uint8 u8Comm_RequestSCIPacket (void)
{
    uint8 u8counter;
    uint8 u8timeout;
    uint8 u8checksum = 0;
    static uint8 u8tempbuffer;
    
    (void)u8SCI1_RxPending();
    (void)u8SCI1_ReadByte();
    
    vfnSCI1_SendByte (COMM_REQUEST_PACKET1);

    while(!u8SCI1_TxBufferEmpty());
    
    u8timeout = u8TPM_GetTimerCounterH(SCHEDULER_TIMER);
        
    while(!u8SCI1_RxPending() && (u8timeout < 7)) 
    {
        u8timeout = u8TPM_GetTimerCounterH(SCHEDULER_TIMER);
    }
     
    for (u8counter = 0; u8counter < COMM_SENSOR_PACKET_SIZE; u8counter++)
    {
        // Packet request timed out, transceiver is busy or disconnected
        // return error
        if (u8timeout == 7)
        {
            return 0;
        }
       
        u8Comm_SensorPacket[u8counter] = u8SCI1_ReadByte ();
        u8checksum ^= u8Comm_SensorPacket[u8counter];
        
        // Wait for next byte to be received or timeout to happen
        while(!u8SCI1_RxPending() && (u8timeout < 7))
        {
            u8timeout = u8TPM_GetTimerCounterH(SCHEDULER_TIMER);
        }
               
    }
    
    u8tempbuffer = u8SCI1_ReadByte();
    
    // If the checksums don't match, return error
    if (u8checksum != u8tempbuffer)
    {
        return 0;    
    }
    
    // If reception was correct, return success
    return 1; 

}

void vfnComm_SendSCICommand (uint8 u8command)
{
    vfnSCI1_SendByte (u8command);

    while(!u8SCI1_TxComplete());   
}

void vfnComm_BuildSMACPacket (void)
{
    uint8 u8tempo;
    u8Comm_TxSMACPacket[0] = (uint8)(u16Comm_CurrentMACAddr >> 8);
    
    // Hope compiler is smart enough to work this out fast enough
    u8Comm_TxSMACPacket[1] = (uint8)(u16Comm_CurrentMACAddr & 0x00FF);
    
    u8Comm_TxSMACPacket[2] = (u8Comm_CurrentSMACMsg);
    
    if( u8ControllerStatus & CSTATUS_MOTOR_ON)
	{    
    	if(u8ExtraAirplaneSettings & AUTOMATIC_PILOT_ON)
    	{
    		u8Comm_TxSMACPacket[3] = AUTO_PILOT_DEFAULT_SPEED;
    	}
    	else
    	{
    		//Speed never is sent as 0. Speed sent as 0 means motor status off
    		//which forces plane to exit motor status on if present
    		//Speed slower than 20 is set as 0 by plane
    		u8Comm_TxSMACPacket[3] = 15 + (uint8)(((uint16)u8speed * 13) / 51 ); //from 0-255 to 15-80
    		//15 and 16 puts speed on 0 (this calculus is made by airplane)
    		//17, 18, 19 puts speed on 20
    		//20 - 80 are directly sent to motor
    	/*	if(u8tempo<10)
    		{
    			u8Comm_TxSMACPacket[3] = 1;
    		}
    		else if(u8tempo < 20)
    		{
    			u8Comm_TxSMACPacket[3] = 20;
    		}
    		else
    		{
    			u8Comm_TxSMACPacket[3] = u8tempo;
    		}*/
    	}
    }
    else
    {
    	u8Comm_TxSMACPacket[3] = 0;
    }
    
    if(u8ExtraAirplaneSettings & AUTOMATIC_PILOT_ON)
    {
	    u8Comm_TxSMACPacket[4] = AUTO_PILOT_DEFAULT_AILERON;
	    
	    u8Comm_TxSMACPacket[5] = AUTO_PILOT_DEFAULT_RUDDER;
	    
	    u8Comm_TxSMACPacket[6] = AUTO_PILOT_DEFAULT_ELEVATOR;
	}
	else
	{
	    u8Comm_TxSMACPacket[4] = -u8accel_y;//(uint8)((-((int16)u8accel_y)) + 256);//change from (128-97)-(128+97) to (128+97)-(128-97)
	    
	    u8Comm_TxSMACPacket[5] = u8accel_y;
	    
	    u8Comm_TxSMACPacket[6] = u8accel_x;
	}
	    
    u8Comm_TxSMACPacket[7] = (u8configuration1 );
    
    u8Comm_TxSMACPacket[8] = (u8configuration2 );
    
    u8Comm_TxSMACPacket[9] = (u8configuration3 );
    u8Comm_TxSMACPacket[10] = u8ExtraAirplaneSettings;
    
    //u8Comm_TxSMACPacket[9] = u8ExtraAirplaneSettings;
}

void vfnComm_ReadSMACPacket (void)
{
	static uint8 u8LastPlaneBattery;
	uint8 u8tempo;
    uint16 u16tempo;
    
	u16DistanceToAirplane = u8Comm_RxSMACPacket[3];
	u16DistanceToAirplane <<= 8;
	u16DistanceToAirplane += u8Comm_RxSMACPacket[4];
	
	u8Directions = u8Comm_RxSMACPacket[5];
	
	u8PlaneSpeed = u8Comm_RxSMACPacket[6];
	
	u16Altitude = u8Comm_RxSMACPacket[7];
	u16Altitude <<= 8;
	u16Altitude += u8Comm_RxSMACPacket[8];
	
	//u8batteryPlane = u8Comm_RxSMACPacket[9];
	if(u8Comm_RxSMACPacket[9] < 174)
	{
	   	u8batteryPlane=0;
	}
	else if(u8Comm_RxSMACPacket[9]>239)
	{
	   	u8batteryPlane=100;
	}
	else if(u8LastPlaneBattery < u8Comm_RxSMACPacket[9]) //One way hysteresis for battery
    {
    	u8tempo = u8Comm_RxSMACPacket[9] - u8LastPlaneBattery;
    }
    else
    {
    	u8tempo = 2;
    }
    
    if(u8tempo>1)
	{    
	    u8tempo=u8Comm_RxSMACPacket[9];
	    u8LastPlaneBattery = u8Comm_RxSMACPacket[9];
	    
	    u8tempo = u8tempo - 174;
	    u16tempo = u8tempo * 100;
	    u8batteryPlane = (uint8)(u16tempo / 65);	
    }
    else
    {
    	//Level is no updated		
    } 
	

	u8Ambientemperature = u8Comm_RxSMACPacket[10];
	
	if(u8Ambientemperature < 50)
	{
		_asm NOP;
	}
	
	u8AirplaneGeneralStatus = u8Comm_RxSMACPacket[11];
	
	if ((u8Comm_RxSMACPacket[12] & 0xF0) == 0xA0)
	{
		u8PlaneConfAnswerFailure |= 1;
	}
	
	if ((u8Comm_RxSMACPacket[13] & 0xF0) == 0xA0)
	{
		u8PlaneConfAnswerFailure |= 1;
	}
	
	if ((u8Comm_RxSMACPacket[14] & 0xC0) == 0x80)
	{
		u8PlaneConfAnswerFailure |= 1;
	}
	
	u16PlaneConfAnswer |= (u8Comm_RxSMACPacket[12] & 0x0F)<<4;
	
	//u8PlaneConfAnswer <<= 4;
	
	u16PlaneConfAnswer |= (u8Comm_RxSMACPacket[13] & 0x0F);			
	
	u16PlaneConfAnswer |= (u8Comm_RxSMACPacket[14] & 0x3F)<<8;
	
}



